﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using Drifting.MotionPlanning.Systems;
using Drifting.Graphics.EffectObjects;
using Drifting.Graphics.PhysicalObjects.Vehicles;
using Drifting.Foundation;
using Microsoft.Xna.Framework;
using Microsoft.Xna.Framework.Graphics;
using Microsoft.Xna.Framework.Input;

using Drifting.MotionPlanning.MultiThreading.Planners;
using Drifting.MotionPlanning.Primitives;
using Drifting.Graphics.Visualizers;

namespace Drifting.Scenes
{
    public class Comparison_SST : Basic
    {
        SST sst;

        public Comparison_SST(ref Matrix view, ref Matrix projection, GraphicsDevice device, Camera camera, Game1 game)
            : base(ref view, ref projection, device, camera, game)
        {
            camera.Camera_InitView(0.001f, 10, 0, 0, 0, 0, (float)(Math.PI / 3.2f), ref view, ref projection);

            BaseSystem point2D = new Point2D();
            Skin_Point2D point_visual = new Skin_Point2D(new List<State>(), device, point2D.SimpleEnv);

            double[] q_zero = new double[] { 3.5, -3f };
            sst = new SST(
                point2D,
                q_zero,
                TextAssistant.LoadFile("../../../MotionPlanning/Resources/SST_Point2D.kdl"),
                inXgoal,
                device,
                point_visual);

            point_visual.SetPlanner(sst);

            grid.XYZ = false;

            sst.AgentNumber = 5;
            sst.Viewable = true;
            sst.Start(100000000); // 100 million
        }
        
        override public bool inXgoal(double[] q)
        {
            if (q[0] < 4.25)
                return false;
            if (q[0] > 4.75)
                return false;

            if (q[1] < 4.25)
                return false;
            if (q[1] > 4.75)
                return false;

            return true;
        }

        override public Color BackgroundColor()
        {
            return Color.White;
        }

        override public void Draw(Matrix camera, Matrix projection)
        {
            base.Draw(camera, projection);

            sst.Skin.Draw(camera, projection);
        }
    }
}
